// Do not remove the include below
#include "Slottimer.h"
#include "Player.h"
#include "GraphicLCD.h"
#include "Button.h"

Player player[4];
Button EnterButton(EnterButtonPin);
Button SelectLeftButton(SelectButtonLeftPin);
Button SelectRightButton(SelectButtonRightPin);
Button StopButton(CancelButtonPin);

GraphicLCD lcd;		// Display on Serial1, Pin
SoftwareSerial serialtoPC(0,1);	// communication to PC



//The setup function is called once at startup of the sketch
void setup()
{
	// Add your initialization code here
	RaceStatus = RS_MENU;
	MenuIndex  = 0;
	fastestLapTime = 1000000;
	lapsToRace = 5;
	timeToRace = 5;

	// LED onboard + extern pin13
	pinMode(indicatorLED, OUTPUT);

	// LCD Setup
	lcd.setLCDBaudRate(115200);
	lcd.setBackgroundBrightness(25);
	lcd.clearScreen();
	lcd.printat(0, 62, "Slotcar-Timing");
	delay(2000);
	drawMenuFrame(RS_MENU);
	showMenuIndex(MenuIndex);

	for (int i=0; i<4; i++)
	{
		player[i].ResetPlayer();
		player[i].nr = i;
	}
	// initialize serial communication:
	serialtoPC.begin(115200);
	// initialize the output pins:
	for (int thisPin = 36; thisPin < 51; thisPin += 2)
	{
		pinMode(thisPin, OUTPUT);
	}
	pinMode(13, OUTPUT);
	delay(100);
	digitalWrite(indicatorLED, LOW);
	digitalWrite(jumpstartLED, LOW);

	//irq pins - set as input and switch on internal pull up
	pinMode(2, INPUT);
	digitalWrite(2, HIGH);
	pinMode(3, INPUT);
	digitalWrite(3, HIGH);
	pinMode(18, INPUT);
	digitalWrite(18, HIGH);
	pinMode(19, INPUT);
	digitalWrite(19, HIGH);

}

// interrupt routine - Slot 1 Detector
void irSlot1()
{
	if (RaceStatus == RS_STOPPED) return;
	player[0].slotsensor = true;
	player[0].timeNow = millis();
	if (RaceStatus == RS_STARTED)
	{
		player[0].playerstatus = JUMPSTART;
	}
}

// interrupt routine - Slot 2 Detector
void irSlot2()
{
	if (RaceStatus == RS_STOPPED) return;
	player[1].slotsensor = true;
	player[1].timeNow = millis();
	if (RaceStatus == RS_STARTED)
	{
		player[1].playerstatus = JUMPSTART;
	}
}

// interrupt routine - Slot 3 Detector
void irSlot3()
{
	if (RaceStatus == RS_STOPPED) return;
	player[2].slotsensor = true;
	player[2].timeNow = millis();
	if (RaceStatus == RS_STARTED)
	{
		player[2].playerstatus = JUMPSTART;
	}
}

// interrupt routine - Slot 4 Detector
void irSlot4()
{
	if (RaceStatus == RS_STOPPED) return;
	player[3].slotsensor = true;
	player[3].timeNow = millis();
	if (RaceStatus == RS_STARTED)
	{
		player[3].playerstatus = JUMPSTART;
	}
}

void setfastestLap(unsigned long oldfastestLapTime)
{
	unsigned long lt1, lt2, newfastestLapTime;
	int pnr0_1, pnr2_3;
	char cBuf[30];
	memset (cBuf, 0, sizeof(cBuf));

	if(player[0].fastestLapTime < player[1].fastestLapTime)
	{
		lt1 = player[0].fastestLapTime;
		pnr0_1 = 0;
	}
	else
	{
		lt1 = player[1].fastestLapTime;
		pnr0_1 = 1;
	}
	if(player[2].fastestLapTime < player[3].fastestLapTime)
	{
		lt2 = player[2].fastestLapTime;
		pnr2_3 = 2;
	}
	else
	{
		lt2 = player[3].fastestLapTime;
		pnr2_3 = 3;
	}
	if(lt1 < lt2)
	{
		newfastestLapTime = lt1;
		playernr = pnr0_1;
	}
	else
	{
		newfastestLapTime = lt2;
		playernr = pnr2_3;
	}

	if (newfastestLapTime < oldfastestLapTime)
	{
		for (int i = 0; i < 4; i++) player[i].fastestPlayer = false;
		fastestLapTime = newfastestLapTime;
		player[playernr].fastestPlayer = true;
		sprintf(cBuf, "Best Lap: Player %1d", playernr + 1);
		lcd.printat(0, 10, "                       ");
		lcd.printat(0, 10, cBuf);
	}
	return;
}

void showfastestLap()
{
	for (int i = 0; i < 4; i++)
	{
		if (player[i].fastestPlayer == true)
		{
			char cBuf[30]; memset(cBuf, 0, sizeof(cBuf));
			sprintf(cBuf, "   << P%1d: %2lu.%03lu >>  ", i+1, fastestLapTime/1000L, fastestLapTime%1000L);
			lcd.printat(0, 8, cBuf);
			delay(3000);
			lcd.printat(0, 8, "                      ");
			serialtoPC.print(cBuf);
		}
	}
}

// Switch on or off Signal lights
void Christmas_tree(bool on)
{
	// todo: add F1-style lights
	if(on)
	{
		lcd.clearScreen();
		digitalWrite(redLED, HIGH);
		lcd.printat(15, 50, "Ready..");
		delay(1500);
		digitalWrite(yellowLED, HIGH);
		lcd.printat(60, 50, "Set..");
		delay(1500);
		digitalWrite(redLED, LOW);
		digitalWrite(yellowLED, LOW);
		digitalWrite(greenLED, HIGH);
		lcd.printat(5, 50, "    <<<  GO  >>>   ");
	}
	else
	{
		digitalWrite(redLED, LOW);
		digitalWrite(yellowLED, LOW);
		digitalWrite(greenLED, LOW);
		digitalWrite(jumpstartLED, LOW);
		lcd.printat(0, 10, "   Race stopped!   ");
		delay(200);
	}
}

void drawMenuFrame(int racestatus)
{
	switch(racestatus)
	{
		case RS_MENU:
			lcd.clearScreen();
			lcd.drawBox(0,10,120,52,1);
			lcd.printat(14,48, "START RACE");
			lcd.printat(14,40, "SHOW FASTEST LAP");
			lcd.printat(14,32, "SETUP MENU");
			lcd.printat(4, 8, "SEL  ---- ---- ENTER");
			break;
		case RS_RACEMENU:
			lcd.clearScreen();
			lcd.drawBox(0,10,120,52,1);
			lcd.printat(14,48, "START RACE - LAPS");
			lcd.printat(14,40, "START RACE - TIME");
			lcd.printat(4, 8, "SEL  ---- BACK ENTER");
			break;
		case RS_SETUPMENU:
			lcd.clearScreen();
			lcd.drawBox(0,10,120,52,1);
			lcd.printat(14,48, "SET LAPS");
			lcd.printat(14,40, "SET TIME");
			lcd.printat(14,32, "RESET DATA");
			lcd.printat(4, 8, "LAPS TIME RST  BACK");
			break;
		default:
			break;
	}
}

void showMenuIndex(int index)
{
	switch(index)
	{
		case 0:
			lcd.drawCircle(4,44,1,1);
			lcd.drawCircle(4,36,1,0);
			lcd.drawCircle(4,28,1,0);
			lcd.drawCircle(4,20,1,0);
			break;
		case 1:
			lcd.drawCircle(4,44,1,0);
			lcd.drawCircle(4,36,1,1);
			lcd.drawCircle(4,28,1,0);
			lcd.drawCircle(4,20,1,0);
			break;
		case 2:
			lcd.drawCircle(4,44,1,0);
			lcd.drawCircle(4,36,1,0);
			lcd.drawCircle(4,28,1,1);
			lcd.drawCircle(4,20,1,0);
			break;
		case 3:
			lcd.drawCircle(4,44,1,0);
			lcd.drawCircle(4,36,1,0);
			lcd.drawCircle(4,28,1,0);
			lcd.drawCircle(4,20,1,1);
			break;
	}
}

void showSetupData(int laps, int time)
{
	char clap[4], ctime[4];
	memset(clap, 0, 4);
	memset(ctime, 0, 4);

	sprintf(clap,  "%2d", laps);
	sprintf(ctime, "%2d", time);
	lcd.printat(80,48, "    ");
	lcd.printat(80,40, "    ");
	lcd.printat(80,48, clap);
	lcd.printat(80,40, ctime);
}

void showRaceResults(int index)
{
	char buf[40];
	memset(buf, 0, sizeof(buf));

	sprintf(buf,  "Winner: Player %1d", index+1);

	lcd.printat(14,60, "                     ");
	lcd.printat(14,60, buf);

}

void updateRace()
{
	unsigned long laptime = 0;
	char playerdata[50];

	for (int index = 0; index < 4; index++)
	{
		if (player[index].slotsensor)
		{
			if (player[index].lapCount > 0)
			{
				laptime = player[index].timeNow - player[index].startTime;
				if (laptime < shortestPossibleLaptime)	// cancel impossible laptimes - false detection
				{
					player[index].slotsensor = false;
					return;  // false sensor
				}
				if(laptime < player[index].fastestLapTime)	// new fastest Lap?
				{
					player[index].fastestLapTime = laptime;
				}
				player[index].lapTime = laptime;	// new laptime
				player[index].startTime = player[index].timeNow;
				player[index].getPlayerStats(playerdata, DATA2PC);
				serialtoPC.print(playerdata);  // player stats message to PC
				player[index].getPlayerStats(playerdata, LCDDISPLAY);	// player stats on display

				if (player[index].lapCount >= lapsToRace)	// race finished?
				{
					player[index].racePosition = position++;
					//serialtoPC.print("");
					detachInterrupt(0);  // Pin 2
					detachInterrupt(1);  // Pin 3
					detachInterrupt(4);  // Pin 19
					detachInterrupt(5);  // Pin 18
					showRaceResults(index);
					RaceStatus = RS_FINISHED;
				}

				// print playerdata on display
				switch(player[index].nr)
				{
					case 0:
						lcd.printat(0,50,playerdata);
						break;
					case 1:
						lcd.printat(0,40,playerdata);
						break;
					case 2:
						lcd.printat(0,30,playerdata);
						break;
					case 3:
						lcd.printat(0,20,playerdata);
						break;
				}
			}
			else	// first lap after start
			{
				player[index].startTime = player[index].timeNow;	// store timestamp of first time passing the sensor
			}
			player[index].lapCount++;
			player[index].slotsensor = false;
			for (int cnt = 0; cnt < 1; cnt++)
			{
				// fire indicator for passing start/finish line (audio/visual)
				digitalWrite(indicatorLED, HIGH);
				delay(50);
				digitalWrite(indicatorLED, LOW);
				delay(50);
			}
		}
	}
}

// The loop function is called in an endless loop
void loop()
{
//Add your repeated code here
	EnterBtn       = digitalRead(EnterButtonPin);
	SelectBtnLeft  = digitalRead(SelectButtonLeftPin);
	SelectBtnRight = digitalRead(SelectButtonRightPin);
	CancelBtn      = digitalRead(CancelButtonPin);

	switch (RaceStatus)
	{
		case RS_STOPPED:	// Race stopped, draw menu
			if(SelectBtnLeft || SelectBtnRight || CancelBtn)
			{
				detachInterrupt(0);  // Pin 2
				detachInterrupt(1);  // Pin 3
				detachInterrupt(4);  // Pin 19
				detachInterrupt(5);  // Pin 18
				drawMenuFrame(RS_MENU);
				showMenuIndex(MenuIndex);
				RaceStatus = RS_MENU;
			}
			if(EnterBtn)
			{
				RaceStatus = RS_STARTED;
				player[0].playerstatus = player[1].playerstatus = player[2].playerstatus = player[3].playerstatus = RACE;
			}
			break;
		case RS_STARTED:	// Race started, prepare christmastree, check for jumpstart
			// New race - reset all player data
			fastestLapTime = 1000000;
			position = 0;
			player[0].ResetPlayer();
			player[1].ResetPlayer();
			player[2].ResetPlayer();
			player[3].ResetPlayer();

			attachInterrupt(0, irSlot1, FALLING);  // Pin 2
			attachInterrupt(1, irSlot2, FALLING);  // Pin 3
			attachInterrupt(4, irSlot3, FALLING);  // Pin 18
			attachInterrupt(5, irSlot4, FALLING);  // Pin 19
			Christmas_tree(true);			// red-yellow-green-goo!
			for(int i=0; i<4; i++)
			{
				if (player[i].CheckForJumpStart())
				{
					player[i].playerstatus = JUMPSTART;
					digitalWrite(jumpstartLED, HIGH);
					// todo: jumpstart 
				}
			}
			RaceStatus = RS_RUNNING;
			lcd.printat(25,10, "Race running..");
			break;

		case RS_RUNNING:	// Race is running
			if(CancelBtn)
			{
				RaceStatus = RS_STOPPED;
				player[0].playerstatus = player[1].playerstatus = player[2].playerstatus = player[3].playerstatus = STOP;
				Christmas_tree(false);
				delay(200);
			}
			updateRace();
			digitalWrite(greenLED, HIGH);
			delay(50);
			digitalWrite(greenLED, LOW);
			delay(50);
			setfastestLap(fastestLapTime);
			break;

		case RS_FINISHED:	// Race finished, show Results
			if (CancelBtn || SelectBtnLeft || SelectBtnRight || EnterBtn) RaceStatus = RS_MENU;
			break;

		case RS_MENU:	// Show Main-Menu
			if (SelectBtnRight)		// goto next line
			{
				MenuIndex += 1;
				if (MenuIndex > 2) MenuIndex = 0;
				showMenuIndex(MenuIndex);
				delay(200);
			}
			if (EnterBtn)		// Selection
			{
				switch (MenuIndex)
				{
					case 0:		// Show Race-Menu
						RaceStatus = RS_RACEMENU;
						MenuIndex = 0;
						drawMenuFrame(RS_RACEMENU);
						showMenuIndex(MenuIndex);
						delay(200);
						break;
					case 1:		// Show fastest driven Lap
						showfastestLap();
						delay(200);
						break;
					case 2:		// Setup Menu
						RaceStatus = RS_SETUPMENU;
						MenuIndex = 0;
						drawMenuFrame(RS_SETUPMENU);
						showSetupData(lapsToRace, timeToRace);
						delay(200);
						break;
					default:
						break;
				}
			}
			break;
		case RS_RACEMENU:	// Show Race-Menu (Race count laps, race count time, ...)
			if (SelectBtnRight)
			{
				MenuIndex += 1;
				if (MenuIndex > 1) MenuIndex = 0;
				showMenuIndex(MenuIndex);
				delay(200);
			}
			if (EnterBtn)
			{
				switch (MenuIndex)
				{
					case 0:	// Start race - laps
						RaceStatus = RS_STARTED;
						racetype = RT_LAPS;
						MenuIndex = 0;
						break;
					case 1:	// start race - time
						// todo
						racetype = RT_TIME;
						break;
					default:
						break;
				}
				delay(200);
			}
			if (CancelBtn)
			{
				RaceStatus = RS_MENU;
				MenuIndex = 0;
				drawMenuFrame(RS_MENU);
				showMenuIndex(MenuIndex);
				delay(200);
			}
			break;
		default:
			break;
		case RS_SETUPMENU:
			if (SelectBtnRight)
			{
				lapsToRace += 5;
				if (lapsToRace > 20) lapsToRace = 5;
				showSetupData(lapsToRace, timeToRace);
				delay(200);
			}
			if (SelectBtnLeft)
			{
				timeToRace += 5;
				if (timeToRace > 20) timeToRace = 5;
				showSetupData(lapsToRace, timeToRace);
				delay(200);
			}
			if (EnterBtn)
			{
				RaceStatus = RS_MENU;
				MenuIndex = 0;
				drawMenuFrame(RS_MENU);
				showMenuIndex(MenuIndex);
				delay(200);
			}
			if (CancelBtn)
			{
				// Reset all Racedata
				player[0].ResetPlayer();
				player[1].ResetPlayer();
				player[2].ResetPlayer();
				player[3].ResetPlayer();
				fastestLapTime = 1000000;
				lcd.printat(14, 24, "Race Data deleted");
				delay(2000);
				lcd.printat(14, 24, "                 ");
				break;

			}
			break;
	}
}
